<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>Aria: gotoActionExample.cpp</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="resize.js"></script>
<script type="text/javascript" src="navtreedata.js"></script>
<script type="text/javascript" src="navtree.js"></script>
<script type="text/javascript">
  $(document).ready(initResizable);
  $(window).load(resizeHeight);
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">Aria
   &#160;<span id="projectnumber">2.9.4</span>
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="pages.html"><span>Related&#160;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li><a href="files.html"><span>Files</span></a></li>
      <li><a href="examples.html"><span>Examples</span></a></li>
    </ul>
  </div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
  <div id="nav-tree">
    <div id="nav-tree-contents">
      <div id="nav-sync" class="sync"></div>
    </div>
  </div>
  <div id="splitbar" style="-moz-user-select:none;" 
       class="ui-resizable-handle">
  </div>
</div>
<script type="text/javascript">
$(document).ready(function(){initNavTree('gotoActionExample_8cpp-example.html','');});
</script>
<div id="doc-content">
<div class="header">
  <div class="headertitle">
<div class="title">gotoActionExample.cpp</div>  </div>
</div><!--header-->
<div class="contents">
<p>Uses <a class="el" href="classArActionGoto.html" title="This action goes to a given ArPose very naively. ">ArActionGoto</a> or <a class="el" href="classArActionGotoStraight.html" title="This action sets a heading and forward velocity based on angle and distance from current robot positi...">ArActionGotoStraight</a> (modify code to switch) to drive the robot in a squareThis program will make the robot drive in a 2.5x2.5 meter square by setting each corner in turn as the goal for the action. It also uses speed limiting actions to avoid collisions. After some time, it cancels the goal (and the robot stops due to a stopping action) and exits.</p>
<p>You can modify the code below to change the type of <code>gotoPoseAction</code> to either <a class="el" href="classArActionGotoStraight.html" title="This action sets a heading and forward velocity based on angle and distance from current robot positi...">ArActionGotoStraight</a> or <a class="el" href="classArActionGoto.html" title="This action goes to a given ArPose very naively. ">ArActionGoto</a> to compare these if you wish. (These are slightly different actions with similar interfaces; generally <a class="el" href="classArActionGotoStraight.html" title="This action sets a heading and forward velocity based on angle and distance from current robot positi...">ArActionGotoStraight</a> is preferred, and has a few more options.)</p>
<div class="fragment"><div class="line"><span class="preprocessor">#include &quot;Aria.h&quot;</span></div><div class="line"></div><div class="line"><span class="keywordtype">int</span> main(<span class="keywordtype">int</span> argc, <span class="keywordtype">char</span> **argv)</div><div class="line">{</div><div class="line">  <a name="a0"></a><a class="code" href="classAria.html#ad99c16b5d947229d9f8e1c5b2d4cdd73">Aria::init</a>();</div><div class="line">  <a name="_a1"></a><a class="code" href="classArArgumentParser.html">ArArgumentParser</a> parser(&amp;argc, argv);</div><div class="line">  parser.<a name="a2"></a><a class="code" href="classArArgumentParser.html#a501694b62a7f8b0c3cadd84aeafe1b35">loadDefaultArguments</a>();</div><div class="line">  <a name="_a3"></a><a class="code" href="classArRobot.html">ArRobot</a> robot;</div><div class="line">  <a name="_a4"></a><a class="code" href="classArAnalogGyro.html">ArAnalogGyro</a> gyro(&amp;robot);</div><div class="line">  <a name="_a5"></a><a class="code" href="classArSonarDevice.html">ArSonarDevice</a> sonar;</div><div class="line">  <a name="_a6"></a><a class="code" href="classArRobotConnector.html">ArRobotConnector</a> robotConnector(&amp;parser, &amp;robot);</div><div class="line">  <a name="_a7"></a><a class="code" href="classArLaserConnector.html">ArLaserConnector</a> laserConnector(&amp;parser, &amp;robot, &amp;robotConnector);</div><div class="line"></div><div class="line"></div><div class="line">  <span class="comment">// Connect to the robot, get some initial data from it such as type and name,</span></div><div class="line">  <span class="comment">// and then load parameter files for this robot.</span></div><div class="line">  <span class="keywordflow">if</span>(!robotConnector.<a name="a8"></a><a class="code" href="classArRobotConnector.html#ab192ac393d9cc1e5b61b1a82b08d085d">connectRobot</a>())</div><div class="line">  {</div><div class="line">    <a name="a9"></a><a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a name="a10"></a><a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a012daf6573594f91242f8dd7c02eb74b">ArLog::Terse</a>, <span class="stringliteral">&quot;gotoActionExample: Could not connect to the robot.&quot;</span>);</div><div class="line">    <span class="keywordflow">if</span>(parser.<a name="a11"></a><a class="code" href="classArArgumentParser.html#ac3889cbdfa862031c51bf80e06954914">checkHelpAndWarnUnparsed</a>())</div><div class="line">    {</div><div class="line">        <span class="comment">// -help not given</span></div><div class="line">        <a name="a12"></a><a class="code" href="classAria.html#a10d71f3d4d0cf7f38c58a1f749f64a42">Aria::logOptions</a>();</div><div class="line">        <a name="a13"></a><a class="code" href="classAria.html#a6abc3d39b3e9548063bb8e78375acc17">Aria::exit</a>(1);</div><div class="line">    }</div><div class="line">  }</div><div class="line"></div><div class="line">  <span class="keywordflow">if</span> (!<a name="a14"></a><a class="code" href="classAria.html#a1b090c01d88bb420b8cf8e0384d25ee1">Aria::parseArgs</a>() || !parser.<a class="code" href="classArArgumentParser.html#ac3889cbdfa862031c51bf80e06954914">checkHelpAndWarnUnparsed</a>())</div><div class="line">  {</div><div class="line">    <a class="code" href="classAria.html#a10d71f3d4d0cf7f38c58a1f749f64a42">Aria::logOptions</a>();</div><div class="line">    <a class="code" href="classAria.html#a6abc3d39b3e9548063bb8e78375acc17">Aria::exit</a>(1);</div><div class="line">  }</div><div class="line"></div><div class="line">  <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a name="a15"></a><a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;gotoActionExample: Connected to robot.&quot;</span>);</div><div class="line"></div><div class="line">  robot.<a name="a16"></a><a class="code" href="classArRobot.html#a97d57b28549fdef851c00749a97cc776">addRangeDevice</a>(&amp;sonar);</div><div class="line">  robot.<a name="a17"></a><a class="code" href="classArRobot.html#a3c3d878cfd11a2ef91230005a83bfcaa">runAsync</a>(<span class="keyword">true</span>);</div><div class="line"></div><div class="line">  <span class="comment">// Collision avoidance actions at higher priority</span></div><div class="line">  <a name="_a18"></a><a class="code" href="classArActionLimiterForwards.html">ArActionLimiterForwards</a> limiterAction(<span class="stringliteral">&quot;speed limiter near&quot;</span>, 300, 600, 250);</div><div class="line">  <a class="code" href="classArActionLimiterForwards.html">ArActionLimiterForwards</a> limiterFarAction(<span class="stringliteral">&quot;speed limiter far&quot;</span>, 300, 1100, 400);</div><div class="line">  <a name="_a19"></a><a class="code" href="classArActionLimiterTableSensor.html">ArActionLimiterTableSensor</a> tableLimiterAction;</div><div class="line">  robot.<a name="a20"></a><a class="code" href="group__easy.html#ga9ab030786d423237d3e4aa06c0e9d8ce">addAction</a>(&amp;tableLimiterAction, 100);</div><div class="line">  robot.<a class="code" href="group__easy.html#ga9ab030786d423237d3e4aa06c0e9d8ce">addAction</a>(&amp;limiterAction, 95);</div><div class="line">  robot.<a class="code" href="group__easy.html#ga9ab030786d423237d3e4aa06c0e9d8ce">addAction</a>(&amp;limiterFarAction, 90);</div><div class="line"></div><div class="line">  <span class="comment">// Goto action at lower priority</span></div><div class="line">  <span class="comment">//ArActionGoto gotoPoseAction(&quot;goto&quot;);</span></div><div class="line">  <a name="_a21"></a><a class="code" href="classArActionGotoStraight.html">ArActionGotoStraight</a> gotoPoseAction(<span class="stringliteral">&quot;gotostraight&quot;</span>);</div><div class="line">  robot.<a class="code" href="group__easy.html#ga9ab030786d423237d3e4aa06c0e9d8ce">addAction</a>(&amp;gotoPoseAction, 50);</div><div class="line">  </div><div class="line">  <span class="comment">// Stop action at lower priority, so the robot stops if it has no goal</span></div><div class="line">  <a name="_a22"></a><a class="code" href="classArActionStop.html">ArActionStop</a> stopAction(<span class="stringliteral">&quot;stop&quot;</span>);</div><div class="line">  robot.<a class="code" href="group__easy.html#ga9ab030786d423237d3e4aa06c0e9d8ce">addAction</a>(&amp;stopAction, 40);</div><div class="line"></div><div class="line"></div><div class="line">  <span class="comment">// turn on the motors, turn off amigobot sounds</span></div><div class="line">  robot.<a name="a23"></a><a class="code" href="group__easy.html#gaa3657bebab0086dd14ed0c8c3f76dc8b">enableMotors</a>();</div><div class="line">  robot.<a name="a24"></a><a class="code" href="classArRobot.html#aceef80a6274fb34aafcc422e4b1778c1">comInt</a>(<a name="a25"></a><a class="code" href="classArCommands.html#a9a31f904a95bbc31d8b33c6637100cc4a7a256695b7937192e88c976d2fd106e5">ArCommands::SOUNDTOG</a>, 0);</div><div class="line"></div><div class="line">  <span class="keyword">const</span> <span class="keywordtype">int</span> duration = 30000; <span class="comment">//msec</span></div><div class="line">  <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;Going to four goals in turn for %d seconds, then cancelling goal and exiting.&quot;</span>, duration/1000);</div><div class="line"></div><div class="line">  <span class="keywordtype">bool</span> first = <span class="keyword">true</span>;</div><div class="line">  <span class="keywordtype">int</span> goalNum = 0;</div><div class="line">  <a name="_a26"></a><a class="code" href="classArTime.html">ArTime</a> start;</div><div class="line">  start.<a name="a27"></a><a class="code" href="group__easy.html#ga7c269e161c8e7cb56bde73dea501a619">setToNow</a>();</div><div class="line">  <span class="keywordflow">while</span> (<a name="a28"></a><a class="code" href="classAria.html#a573c386c42739be20fa97a99ae65f9ee">Aria::getRunning</a>()) </div><div class="line">  {</div><div class="line">    robot.<a name="a29"></a><a class="code" href="classArRobot.html#a5aecc7ac998c9a31b82fdb904efa8609">lock</a>();</div><div class="line"></div><div class="line">    <span class="comment">// Choose a new goal if this is the first loop iteration, or if we </span></div><div class="line">    <span class="comment">// achieved the previous goal.</span></div><div class="line">    <span class="keywordflow">if</span> (first || gotoPoseAction.<a name="a30"></a><a class="code" href="classArActionGotoStraight.html#a6bd583eb347714097d7cf9a56cbaadeb">haveAchievedGoal</a>())</div><div class="line">    {</div><div class="line">      first = <span class="keyword">false</span>;</div><div class="line">      goalNum++;</div><div class="line">      <span class="keywordflow">if</span> (goalNum &gt; 4)</div><div class="line">        goalNum = 1; <span class="comment">// start again at goal #1</span></div><div class="line"></div><div class="line">      <span class="comment">// set our positions for the different goals</span></div><div class="line">      <span class="keywordflow">if</span> (goalNum == 1)</div><div class="line">        gotoPoseAction.<a name="a31"></a><a class="code" href="classArActionGotoStraight.html#aaf5bf26c33ea8ac9817f0cede8987e48">setGoal</a>(<a name="_a32"></a><a class="code" href="classArPose.html">ArPose</a>(2500, 0));</div><div class="line">      <span class="keywordflow">else</span> <span class="keywordflow">if</span> (goalNum == 2)</div><div class="line">        gotoPoseAction.<a class="code" href="classArActionGotoStraight.html#aaf5bf26c33ea8ac9817f0cede8987e48">setGoal</a>(<a class="code" href="classArPose.html">ArPose</a>(2500, 2500));</div><div class="line">      <span class="keywordflow">else</span> <span class="keywordflow">if</span> (goalNum == 3)</div><div class="line">        gotoPoseAction.<a class="code" href="classArActionGotoStraight.html#aaf5bf26c33ea8ac9817f0cede8987e48">setGoal</a>(<a class="code" href="classArPose.html">ArPose</a>(0, 2500));</div><div class="line">      <span class="keywordflow">else</span> <span class="keywordflow">if</span> (goalNum == 4)</div><div class="line">        gotoPoseAction.<a class="code" href="classArActionGotoStraight.html#aaf5bf26c33ea8ac9817f0cede8987e48">setGoal</a>(<a class="code" href="classArPose.html">ArPose</a>(0, 0));</div><div class="line"></div><div class="line">      <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;Going to next goal at %.0f %.0f&quot;</span>, </div><div class="line">            gotoPoseAction.<a name="a33"></a><a class="code" href="classArActionGotoStraight.html#a1b5285ccf2a2ce6878d742c2c146b4e8">getGoal</a>().<a name="a34"></a><a class="code" href="classArPose.html#afd85bde3b0320d19d108b172c3f332b1">getX</a>(), gotoPoseAction.<a class="code" href="classArActionGotoStraight.html#a1b5285ccf2a2ce6878d742c2c146b4e8">getGoal</a>().<a name="a35"></a><a class="code" href="classArPose.html#a563e9580f307fe832a7269f72bfda8fb">getY</a>());</div><div class="line">    }</div><div class="line"></div><div class="line">    <span class="keywordflow">if</span>(start.<a name="a36"></a><a class="code" href="group__easy.html#gabfac503dbf12dac57c8dc133d0b034a1">mSecSince</a>() &gt;= duration) {</div><div class="line">      <a class="code" href="classArLog.html#a43a8b3789126c818f390f24bdbceccce">ArLog::log</a>(<a class="code" href="classArLog.html#ac8cc0fb3aa323ab2a1c21340fdd1dce3a7040faf60eeb155eaa85d439b1066ca1">ArLog::Normal</a>, <span class="stringliteral">&quot;%d seconds have elapsed. Cancelling current goal, waiting 3 seconds, and exiting.&quot;</span>, duration/1000);</div><div class="line">      gotoPoseAction.<a name="a37"></a><a class="code" href="classArActionGotoStraight.html#a660426998612c5e9219c55e5429d7f2a">cancelGoal</a>();</div><div class="line">      robot.<a name="a38"></a><a class="code" href="classArRobot.html#a403d9de010dc2b8d478cd0b14b2d9491">unlock</a>();</div><div class="line">      <a name="a39"></a><a class="code" href="group__easy.html#ga4dfb878a3f9123b8dd96ae05b2fd94cb">ArUtil::sleep</a>(3000);</div><div class="line">      <span class="keywordflow">break</span>;</div><div class="line">    }</div><div class="line"></div><div class="line">    robot.<a class="code" href="classArRobot.html#a403d9de010dc2b8d478cd0b14b2d9491">unlock</a>();</div><div class="line">    <a class="code" href="group__easy.html#ga4dfb878a3f9123b8dd96ae05b2fd94cb">ArUtil::sleep</a>(100);</div><div class="line">  }</div><div class="line">  </div><div class="line">  <span class="comment">// Robot disconnected or time elapsed, shut down</span></div><div class="line">  <a class="code" href="classAria.html#a6abc3d39b3e9548063bb8e78375acc17">Aria::exit</a>(0);</div><div class="line">  <span class="keywordflow">return</span> 0;</div><div class="line">}</div></div><!-- fragment --> </div><!-- contents -->
</div><!-- doc-content -->
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
  <ul>
    <li class="footer">Generated by
    <a href="http://www.doxygen.org/index.html">
    <img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.8.11 </li>
  </ul>
</div>
</body>
</html>
